import java.io.IOException;
import java.sql.Connection;
import java.sql.DriverManager;
import java.sql.ResultSet;
import java.sql.SQLException;
import java.sql.Statement;

import java.util.LinkedList;

import org.postgis.LineString;
import org.postgis.PGgeometry;
import org.postgis.Point;

/**
 * MapMatchAlgo is act like the processing heart of Map Matching algorithm. The
 * calculation will be done here and return the results consist of linkID and
 * lambda.
 * */

public class MapMatchAlgo {

	public final double PI_DIV_180 = Math.PI / 180;
	public final double PI_MUL_2 = Math.PI * 2;

	private static Connection conn_mainDB;
	private static Statement statement;

	public MapMatchAlgo() {
		try {
			conn_mainDB = DriverManager.getConnection(MainMap.mainDB, "fang",
					"root");
			statement = conn_mainDB.createStatement();
		} catch (SQLException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	public MapMatchResult mapmatch(GPSpnt p) throws SQLException, IOException {
		Object[] output = new Object[2];

		// wrap the inputs
		pntToCurveMapmatch(p, output);

		int output_link_id = (Integer) output[0];
		double output_lambda = (Double) output[1];

		return new MapMatchResult(output_link_id, output_lambda);
	}

	/**
	 * if no candidate exists, meaning matching failed, then the outputObjectArr
	 * is {-1,-1}
	 * 
	 * @param inputObjectArr
	 *            an input array -- lat : double, lng : double, course : int
	 * @param outputObjectArr
	 *            an output array -- link_id : int, lambda : double
	 * @throws IOException
	 */
	private void pntToCurveMapmatch(GPSpnt p, Object[] outputObjectArr)
			throws SQLException, IOException {

		// two outputs initialized
		int link_id = -1;
		double lambda = -1.0;

		double lat = p.lat;
		double lng = p.lng;
		double heading = p.heading;
		Point gpsPoint = new Point(lng, lat);

		Debugging.printlnDebug(String.format("%f, %f, %f", lat, lng, heading),
				false);

		LinkedList<LinkObj> idCandidateList = getLinkWithinRange(lat, lng);
		Debugging.printlnDebug("candidate size = " + idCandidateList.size(),
				false);

		// no candidate -> matching failed
		if (idCandidateList.size() == 0) {
			// System.err.println("no candidate links");
			outputObjectArr[0] = link_id; // linkID = -1
			outputObjectArr[1] = lambda; // lambda = -1
			return;
		}

		// initialized var
		double minDiff = PI_MUL_2;
		double secMinDiff = PI_MUL_2;
		LineString firstNL = null, secNL = null;
		Point fstNL_Start = null, fstNL_End = null, secNL_Start = null, secNL_End = null;
		LinkedList<LineString> lsCandidateList = new LinkedList<LineString>();
		ResultSet r = null;
		// //////////////////

		// ---------------------------------------------------------
		// Write and read into file : gid, the_geom
		// ---------------------------------------------------------
		// ReadWriteGidRoadmotOneway(r);
		// ---------------------------------------------------------

		try {
			for (int i = 0; i < idCandidateList.size(); i++) {

				// System.out.println(idCandidateList.get(i).getGeom());
				PGgeometry geom = (PGgeometry) idCandidateList.get(i).getGeom();
				LineString theLine = (LineString) geom.getGeometry();
				boolean isOneway = "FT".equals(idCandidateList.get(i)
						.getOneway());

				// store LineString retrieved in memory so that we don't bother
				// query
				// them again
				lsCandidateList.add(theLine);

				// the first line in candidate
				if (i == 0) {
					fstNL_Start = theLine.getPoints()[0];
					fstNL_End = theLine.getPoints()[1];
				}

				// find min distance -- node(which is a member of line) to
				// point(identified variable)
				double min = Double.MAX_VALUE;
				int minDistIndex = 0;
				for (int j = 0; j < theLine.numPoints(); j++) {
					double dist = theLine.getPoints()[j].distance(gpsPoint);
					if (dist < min) {
						min = dist;
						minDistIndex = j;
					}
				}

				Point start, end;
				if (minDistIndex != theLine.numPoints() - 1) {

					// pre-assign (presented in Yang's algorithm)
					start = theLine.getPoints()[minDistIndex];
					end = theLine.getPoints()[minDistIndex + 1];

					// FIXME not understand from here
					// adjustment checking (swap: start - end)
					// vector start->gpspoint
					Point vA = new Point(gpsPoint.x - start.x, gpsPoint.y
							- start.y);
					// vector end->gpspoint
					Point vB = new Point(gpsPoint.x - end.x, gpsPoint.y - end.y);
					// vector start->end
					Point vSE = new Point(end.x - start.x, end.y - start.y);
					// vector end->start
					Point vES = new Point(start.x - end.x, start.y - end.y);

					double costheta = getCosTheta(vA, vSE);
					double costheta_ = getCosTheta(vB, vES);

					// crucial criteria here
					if (!(costheta > 0 && costheta_ > 0) && minDistIndex > 0) {
						start = theLine.getPoints()[minDistIndex - 1];
						end = theLine.getPoints()[minDistIndex];
					}
				}
				// for the last vertex of LineString, there is only choice
				else {
					start = theLine.getPoints()[minDistIndex - 1];
					end = theLine.getPoints()[minDistIndex];
				}

				// wrap objects
				Object[] refObject = new Object[8];
				refObject[0] = minDiff;
				refObject[1] = secMinDiff;
				refObject[2] = firstNL;
				refObject[3] = secNL;
				refObject[4] = fstNL_Start;
				refObject[5] = fstNL_End;
				refObject[6] = secNL_Start;
				refObject[7] = secNL_End;

				processMinTheta(gpsPoint, heading, theLine, start, end,
						refObject);

				// if two-way then a swapped SubLineString will also be
				// processed
				if (!isOneway)
					processMinTheta(gpsPoint, heading, theLine, end, start,
							refObject);

				minDiff = (Double) refObject[0];
				secMinDiff = (Double) refObject[1];
				firstNL = (LineString) refObject[2];
				secNL = (LineString) refObject[3];
				fstNL_Start = (Point) refObject[4];
				fstNL_End = (Point) refObject[5];
				secNL_Start = (Point) refObject[6];
				secNL_End = (Point) refObject[7];

			}

			if (firstNL != null)
				Debugging.printlnDebug("#1: " + firstNL.toString(), false);
			if (secNL != null)
				Debugging.printlnDebug("#2: " + secNL.toString(), false);

			double small_deg_angle = 10.0;
			if (secNL != null) {
				if (Math.abs(minDiff - secMinDiff) < small_deg_angle
						* PI_DIV_180) {
					double dist1 = getDistancePointToLine(fstNL_Start,
							fstNL_End, gpsPoint);
					double dist2 = getDistancePointToLine(secNL_Start,
							secNL_End, gpsPoint);
					if (dist2 < dist1) {
						fstNL_Start = secNL_Start;
						fstNL_End = secNL_End;
					}
				}
			}

			// find tentative snapped point
			Point vecA = new Point(fstNL_End.x - fstNL_Start.x, fstNL_End.y
					- fstNL_Start.y);
			Point vecB = new Point(gpsPoint.x - fstNL_Start.x, gpsPoint.y
					- fstNL_Start.y);
			Point proj_vecA_vecB = project(vecA, vecB);
			Point snpPoint = new Point(fstNL_Start.x + proj_vecA_vecB.x,
					fstNL_Start.y + proj_vecA_vecB.y);

			// it's possible that snpPoint calculated here is not correct --
			// just
			// almost correct
			// So we have to find the nearest segment and then do projection
			// again
			LineString subLineString = new LineString();
			LineString theLineString = new LineString();

			// wrap into LineString array
			LineString[] lsInputArr = new LineString[2];
			link_id = findClosestSubLineString(snpPoint, lsInputArr,
					lsCandidateList, idCandidateList);

			// unwrap
			subLineString = lsInputArr[0];
			theLineString = lsInputArr[1];

			// subLineString not exist -> matching failed
			if (subLineString.isEmpty()) {
				Debugging.printlnDebug("subLineString is empty", false);
				outputObjectArr[0] = -1;
				outputObjectArr[1] = -1.0;
				return;
			}

			// snpPoint adjustment
			snpPoint = project(subLineString.getPoint(0),
					subLineString.getPoint(1), snpPoint);

			StringBuffer theLineString_sb = new StringBuffer();
			StringBuffer snpPoint_sb = new StringBuffer();
			theLineString.outerWKT(theLineString_sb);
			snpPoint.outerWKT(snpPoint_sb);
			r = statement
					.executeQuery(String
							.format("select st_line_locate_point(st_setsrid('%s',4326), st_setsrid('%s',4326))",
									theLineString_sb, snpPoint_sb));
			r.next();
			lambda = r.getDouble(1);

		} catch (SQLException e) {
			e.printStackTrace();
		}

		r.close();
		outputObjectArr[0] = link_id;
		outputObjectArr[1] = lambda;
		Debugging.printlnDebug("output: " + link_id + "," + lambda, false);
	}

	// public void ReadWriteGidRoadmotOneway(ResultSet r) throws SQLException,
	// IOException {
	// File f1 = new File(Config.ROADMOT_PATH);
	// File f2 = new File(Config.GID_PATH);
	// ArrayList<String> gid_list = new ArrayList<String>();
	// ArrayList<String> the_geom_list = new ArrayList<String>();
	// ArrayList<String> oneway_list = new ArrayList<String>();
	// // System.out.println(java.lang.Runtime.getRuntime().maxMemory());
	//
	// if (!f1.exists() || !f2.exists()) {
	// WriteFile writeFile;
	// String query_allRoad = String.format(
	// "SELECT gid,the_geom,oneway from \"%s\".\"%s\" ",
	// Config.SCHEMA_NAME_LINK_DATA, Config.TABLE_NAME_LINK_DATA);
	// r = statement.executeQuery(query_allRoad);
	// while (r != null && r.next()) {
	// gid_list.add(r.getString("gid"));
	// // System.out.println(r.getString("gid"));
	// // PGgeometry geom = (PGgeometry) r.getObject("the_geom");
	// // LineString Line = (LineString) geom.getGeometry();
	// the_geom_list.add(r.getString("the_geom"));
	// oneway_list.add(r.getString("oneway"));
	// }
	// writeFile = new WriteFile(gid_list, Config.GID_PATH);
	// writeFile = new WriteFile(the_geom_list, Config.ROADMOT_PATH);
	// writeFile = new WriteFile(oneway_list, Config.ONEWAY_PATH);
	//
	// gid_list.clear();
	// the_geom_list.clear();
	// oneway_list.clear();
	// } else {
	// ReadFile readGid = new ReadFile(Config.GID_PATH);
	// gid_list = readGid.readText();
	// ReadFile readRoadmot = new ReadFile(Config.ROADMOT_PATH);
	// the_geom_list = readRoadmot.readText();
	// ReadFile readOneway = new ReadFile(Config.ONEWAY_PATH);
	// oneway_list = readOneway.readText();
	// }
	//
	// }

	/**
	 * 
	 * @param thePoint
	 * @param subLineString
	 * @param theLineString
	 * @param candidate
	 * @param idList
	 * @return the id associated with the LineString
	 */
	private int findClosestSubLineString(Point thePoint,
			LineString[] lsInputArr, LinkedList<LineString> candidate,
			LinkedList<LinkObj> idList) {
		double minDist = Double.MAX_VALUE;
		int the_id = -1;
		LineString subLineString = new LineString();
		LineString theLineString = new LineString();
		for (int i = 0; i < candidate.size(); i++) {
			LineString curLine = candidate.get(i);
			for (int j = 0; j < curLine.numPoints() - 1; j++) {
				double d = getDistancePointToLine(curLine.getPoint(j),
						curLine.getPoint(j + 1), thePoint);
				if (d < minDist) {
					Point start = curLine.getPoint(j);
					Point end = curLine.getPoint(j + 1);
					Point vA = new Point(thePoint.x - start.x, thePoint.y
							- start.y);
					Point vB = new Point(thePoint.x - end.x, thePoint.y - end.y);
					double costheta = getCosTheta(vA, new Point(
							end.x - start.x, end.y - start.y));
					double costheta_ = getCosTheta(vB, new Point(start.x
							- end.x, start.y - end.y));

					// crucial criteria here
					if (costheta > 0 && costheta_ > 0) {
						Point[] pArr = new Point[2];
						pArr[0] = curLine.getPoint(j);
						pArr[1] = curLine.getPoint(j + 1);
						minDist = d;
						the_id = idList.get(i).getLinkID();
						subLineString = new LineString(pArr);
						theLineString = curLine;
					}
				}
			}
		}

		// wrap them up
		lsInputArr[0] = subLineString;
		lsInputArr[1] = theLineString;

		return the_id;
	}

	/**
	 * project thePoint on the vectec specified by end and start
	 * 
	 * @param start
	 * @param end
	 * @param thePoint
	 * @return
	 */
	private Point project(Point start, Point end, Point thePoint) {
		Point vecA = new Point(end.x - start.x, end.y - start.y);
		Point vecB = new Point(thePoint.x - start.x, thePoint.y - start.y);
		Point proj = project(vecA, vecB);
		return new Point(start.x + proj.x, start.y + proj.y);
	}

	/**
	 * project vector b on vector a
	 * 
	 * @param a
	 *            the basis vector
	 * @param b
	 *            the vector to be projected
	 * @return
	 */
	private Point project(Point a, Point b) {
		double factor = (a.x * b.x + a.y * b.y) / (a.x * a.x + a.y * a.y);
		return new Point(factor * a.x, factor * a.y);
	}

	/**
	 * calculate distance of the input's coordinate
	 * 
	 * @param start
	 * @param end
	 * @param the_point
	 * @return
	 */
	private double getDistancePointToLine(Point start, Point end,
			Point the_point) {
		// Class Point is used as a vector here
		Point vecA = new Point(end.x - start.x, end.y - start.y);
		Point vecB = new Point(the_point.x - start.x, the_point.y - start.y);
		Point proj = project(vecA, vecB);
		Point x = new Point(vecB.x - proj.x, vecB.y - proj.y);
		return x.x * x.x + x.y * x.y;
	}

	/**
	 * 
	 * @param a1
	 *            an angle in azimuth coordinate
	 * @param a2
	 *            another angle in azimuth coordinate
	 * @return diff value in a range of [0,Pi]
	 */
	private double getRadDiffAngle(double a1, double a2) {
		double diff_temp = Math.abs(a1 - a2);
		return Math.min(diff_temp, PI_MUL_2 - diff_temp);
	}

	private double getCosTheta(Point a, Point b) {
		double sizeA_sqr = a.x * a.x + a.y * a.y;
		double sizeB_sqr = b.x * b.x + b.y * b.y;
		double dot = a.x * b.x + a.y * b.y;
		return dot / Math.sqrt(sizeA_sqr * sizeB_sqr);
	}

	/**
	 * 
	 * @param gpsPoint
	 * @param theLine
	 *            current line in candidate
	 * @param start
	 *            current start node
	 * @param end
	 *            current end node
	 * @param refObject
	 *            double minDiff, secMinDiff; LineString firstNL, secNL;
	 *            fstNL_Start,fstNL_End, secNL_Start, secNL_End
	 */
	private void processMinTheta(Point gpsPoint, double heading,
			LineString theLine, Point start, Point end, Object[] refObject) {
		double minDiff = (Double) refObject[0];
		double secMinDiff = (Double) refObject[1];
		LineString firstNL = (LineString) refObject[2];
		LineString secNL = (LineString) refObject[3];
		Point fstNL_Start = (Point) refObject[4];
		Point fstNL_End = (Point) refObject[5];
		Point secNL_Start = (Point) refObject[6];
		Point secNL_End = (Point) refObject[7];

		StringBuffer sbStart = new StringBuffer();
		StringBuffer sbEnd = new StringBuffer();
		start.outerWKT(sbStart);
		end.outerWKT(sbEnd);

		double rad = 0;
		try {
			ResultSet r = statement
					.executeQuery(String
							.format("select st_azimuth(st_setsrid('%s',4326), st_setsrid('%s',4326))",
									sbStart, sbEnd));
			r.next();
			rad = r.getDouble(1);

		} catch (SQLException e) {
			e.printStackTrace();
		}

		// this is absolutely wrong!
		// double diff = Math.abs(rad - heading*Math.PI/180);

		double heading_in_rad = heading * PI_DIV_180;
		double diff = getRadDiffAngle(heading_in_rad, rad);

		// x < #1
		if (diff < minDiff) {
			// #1 -> #2, must be in different linestring
			if (firstNL != null && !start.equals(fstNL_Start)
					&& !end.equals(fstNL_End) && !start.equals(fstNL_End)
					&& !end.equals(fstNL_Start) && !theLine.equals(firstNL)
					&& !theLine.getFirstPoint().equals(firstNL.getFirstPoint())
					&& !theLine.getLastPoint().equals(firstNL.getLastPoint())
					&& !theLine.getLastPoint().equals(firstNL.getFirstPoint())
					&& !theLine.getFirstPoint().equals(firstNL.getLastPoint())) {
				secMinDiff = minDiff;
				secNL = cloneLineString(firstNL);
				secNL_Start = clonePoint(fstNL_Start);
				secNL_End = clonePoint(fstNL_End);
			}
			// #0 -> #1
			minDiff = diff;
			firstNL = cloneLineString(theLine);
			fstNL_Start = clonePoint(start);
			fstNL_End = clonePoint(end);
		} else if (diff > minDiff && diff < secMinDiff
				&& !start.equals(fstNL_Start) && !end.equals(fstNL_End)
				&& !start.equals(fstNL_End) && !end.equals(fstNL_Start)
				&& !theLine.equals(firstNL)
				&& !theLine.getFirstPoint().equals(firstNL.getFirstPoint())
				&& !theLine.getLastPoint().equals(firstNL.getLastPoint())
				&& !theLine.getLastPoint().equals(firstNL.getFirstPoint())
				&& !theLine.getFirstPoint().equals(firstNL.getLastPoint())) {
			secMinDiff = diff;
			secNL = cloneLineString(theLine);
			secNL_Start = clonePoint(start);
			secNL_End = clonePoint(end);
		}

		// wrap object
		refObject[0] = minDiff;
		refObject[1] = secMinDiff;
		refObject[2] = firstNL;
		refObject[3] = secNL;
		refObject[4] = fstNL_Start;
		refObject[5] = fstNL_End;
		refObject[6] = secNL_Start;
		refObject[7] = secNL_End;
	}

	private LineString cloneLineString(LineString original) {
		return new LineString(original.getPoints());
	}

	private Point clonePoint(Point original) {
		return new Point(original.x, original.y);
	}

	private static LinkedList<LinkObj> getLinkWithinRange(double lat, double lng)
			throws SQLException, IOException {
		ResultSet r = null;
		LinkedList<LinkObj> candidateLink = new LinkedList<LinkObj>();

		/**
		 * note: longtitude => x, latitude => y
		 * */

		String sql = String
				.format("select gid,the_geom,oneway from \"%s\".\"%s\" "
						+ " where st_dwithin("
						+ "	st_transform(the_geom, 32647), "
						+ "	st_transform(ST_SetSRID(ST_Point(%f, %f), 4326), 32647), %d) = true",

				Config.SCHEMA_NAME_LINK_DATA, Config.TABLE_NAME_LINK_DATA, lng,
						lat, Config.GPS_ERROR_RADIUS_METER);
		// System.out.println(sql);
		r = statement.executeQuery(sql);
		while (r != null && r.next()) {
			LinkObj l = new LinkObj(r.getInt("gid"),
					(PGgeometry) r.getObject("the_geom"), r.getString("oneway"));
			candidateLink.add(l);
		}
		r.close();

		return candidateLink;
	}

}
